// Copyright 2022 Chen Jun

#include <gtest/gtest.h>

#include <ament_index_cpp/get_package_share_directory.hpp>
#include <opencv2/core/mat.hpp>
#include <opencv2/opencv.hpp>

// STL
#include <algorithm>
#include <chrono>
#include <iostream>
#include <map>
#include <vector>
#include <yaml-cpp/yaml.h>

#include "armor_detector/ArmorDetectorNetwork.h"

using hrc = std::chrono::high_resolution_clock;

TEST(test_nc, benchmark)
{
  YAML::Node yaml = YAML::LoadFile("./config/armor_detector.yaml");
  rm_auto_aim::ArmorDetectorNetwork nc(yaml["img_size_"].as<int>(), yaml["model_path_"].as<std::string>(),
    yaml["enemy_color_"].as<int>(), yaml["score_threshold_"].as<float>(),
    yaml["nms_threshold_"].as<float>());

  cv::Mat test_img = cv::imread("./test/test.jpg");  

  int loop_num = 200;
  int warm_up = 30;

  double time_min = DBL_MAX;
  double time_max = -DBL_MAX;
  double time_avg = 0;

  for (int i = 0; i < warm_up + loop_num; i++) {
    auto start = hrc::now();
    nc.detect(test_img);
    auto end = hrc::now();
    double time = std::chrono::duration<double, std::milli>(end - start).count();
    if (i >= warm_up) {
      time_min = std::min(time_min, time);
      time_max = std::max(time_max, time);
      time_avg += time;
    }
  }
  time_avg /= loop_num;

  std::cout << "time_min: " << time_min << "ms" << std::endl;
  std::cout << "time_max: " << time_max << "ms" << std::endl;
  std::cout << "time_avg: " << time_avg << "ms" << std::endl;
}
